#include "main.h"
#include "ACMMP.h"

void GenerateSampleList(const std::string &dense_folder, std::vector<Problem> &problems)
{
    std::string cluster_list_path = dense_folder + std::string("/pair.txt");

    problems.clear();

    std::ifstream file(cluster_list_path);

    int num_images;
    file >> num_images;

    for (int i = 0; i < num_images; ++i) {
        Problem problem;
        problem.src_image_ids.clear();
        file >> problem.ref_image_id;                   //当前帧id

        int num_src_images;
        file >> num_src_images;
        for (int j = 0; j < num_src_images; ++j) {
            int id;
            float score;
            file >> id >> score;
            if (score <= 0.0f) {
                continue;
            }
            problem.src_image_ids.push_back(id);
        }
        problems.push_back(problem);
    }
}

int ComputeMultiScaleSettings(const std::string &dense_folder, std::vector<Problem> &problems)
{
    int max_num_downscale = -1;
    int size_bound = 1000;
    PatchMatchParams pmp;
    std::string image_folder = dense_folder + std::string("/images");

    size_t num_images = problems.size();

    for (size_t i = 0; i < num_images; ++i) {
        std::stringstream image_path;
        image_path << image_folder << "/" << std::setw(8) << std::setfill('0') << problems[i].ref_image_id << ".jpg";
        cv::Mat_<uint8_t> image_uint = cv::imread(image_path.str(), cv::IMREAD_GRAYSCALE);

        int rows = image_uint.rows;
        int cols = image_uint.cols;
        int max_size = std::max(rows, cols);
        if (max_size > pmp.max_image_size) {
            max_size = pmp.max_image_size;
        }
        problems[i].max_image_size = max_size;

        int k = 0;
        while (max_size > size_bound) {
            max_size /= 2;
            k++;
        }

        if (k > max_num_downscale) {
            max_num_downscale = k;
        }

        problems[i].num_downscale = k;
    }

    return max_num_downscale;
}

//输出图像
/*********************************************************************************************************************************************************/
template <typename T>
T Percentile(const std::vector<T>& elems, const double p) {
  const int idx = static_cast<int>(std::round(p / 100 * (elems.size() - 1)));
  const size_t percentile_idx = std::max(0, std::min(static_cast<int>(elems.size() - 1), idx));
  std::vector<T> ordered_elems = elems;
  std::nth_element(ordered_elems.begin(), ordered_elems.begin() + percentile_idx, ordered_elems.end());
  return ordered_elems.at(percentile_idx);
}
float Interpolate(const float val, const float y0, const float x0, const float y1, const float x1) {
  return (val - x0) * (y1 - y0) / (x1 - x0) + y0;
}
float Base(const float val) {
  if (val <= 0.125f) {
    return 0.0f;
  } else if (val <= 0.375f) {
    return Interpolate(2.0f * val - 1.0f, 0.0f, -0.75f, 1.0f, -0.25f);
  } else if (val <= 0.625f) {
    return 1.0f;
  } else if (val <= 0.87f) {
    return Interpolate(2.0f * val - 1.0f, 1.0f, 0.25f, 0.0f, 0.75f);
  } else {
    return 0.0f;
  }
}
float Red(const float gray) { return Base(gray - 0.25f); }
float Green(const float gray) { return Base(gray); }
float Blue(const float gray) { return Base(gray + 0.25f); }
//输出伪彩图
bool ExportDepthMapByJetColormap(const std::string& fileName, const cv::Mat &depthMap)
{
	std::vector<float> valid_depths;
    for(int i=0 ; i<depthMap.rows ; i++){
        for(int j=0 ; j<depthMap.cols ; j++){
            const float depth = depthMap.at<float>(i,j) ;
			if (depth > 0)
			    valid_depths.push_back(depth);
	    }
    }
	float min_percentile = 2;
	float max_percentile = 98;
	const float robust_depth_min = Percentile(valid_depths, min_percentile);
	const float robust_depth_max = Percentile(valid_depths, max_percentile);
	const float robust_depth_range = robust_depth_max - robust_depth_min;
	cv::Mat img(depthMap.rows,depthMap.cols,CV_8UC3);
	for(int i=0 ; i<depthMap.rows ; i++){
        for(int j=0 ; j<depthMap.cols ; j++){
			const float depth = depthMap.at<float>(i,j);
			if(depth > 0){
				const float robust_depth = std::max(robust_depth_min, std::min(robust_depth_max, depth));
				const float gray = (robust_depth - robust_depth_min) / robust_depth_range;
				img.at<cv::Vec3b>(i,j)[2] = 255 * Red(gray) ;
				img.at<cv::Vec3b>(i,j)[1] = 255 * Green(gray);
				img.at<cv::Vec3b>(i,j)[0] = 255 * Blue(gray);
			}
			else{
				img.at<cv::Vec3b>(i,j)[0] = 0 ;
				img.at<cv::Vec3b>(i,j)[1] = 0 ;
				img.at<cv::Vec3b>(i,j)[2] = 0 ;
			}
		}
	}
	return cv::imwrite(fileName+".png",img);
} 
/*********************************************************************************************************************************************************/

void ProcessProblem(const std::string &dense_folder, const std::vector<Problem> &problems, const int idx, bool geom_consistency, bool planar_prior, bool hierarchy, bool multi_geometrty=false)
{
    const Problem problem = problems[idx];
    std::cout << "Processing image " << std::setw(8) << std::setfill('0') << problem.ref_image_id << "..." << std::endl;
    cudaSetDevice(0);
    std::stringstream result_path;
    result_path << dense_folder << "/ACMMP" << "/2333_" << std::setw(8) << std::setfill('0') << problem.ref_image_id;
    std::string result_folder = result_path.str();
    mkdir(result_folder.c_str(), 0777);

    ACMMP acmmp;
    if (geom_consistency) {
        acmmp.SetGeomConsistencyParams(multi_geometrty);
    }
    if (hierarchy) {
        acmmp.SetHierarchyParams();
    }

    acmmp.InuputInitialization(dense_folder, problems, idx);

    acmmp.CudaSpaceInitialization(dense_folder, problem);
    acmmp.RunPatchMatch();

    const int width = acmmp.GetReferenceImageWidth();
    const int height = acmmp.GetReferenceImageHeight();

    cv::Mat_<float> depths = cv::Mat::zeros(height, width, CV_32FC1);
    cv::Mat_<cv::Vec3f> normals = cv::Mat::zeros(height, width, CV_32FC3);
    cv::Mat_<float> costs = cv::Mat::zeros(height, width, CV_32FC1);

    for (int col = 0; col < width; ++col) {
        for (int row = 0; row < height; ++row) {
            int center = row * width + col;
            float4 plane_hypothesis = acmmp.GetPlaneHypothesis(center);
            depths(row, col) = plane_hypothesis.w;
            normals(row, col) = cv::Vec3f(plane_hypothesis.x, plane_hypothesis.y, plane_hypothesis.z);
            costs(row, col) = acmmp.GetCost(center);
        }
    }

    if (planar_prior) {
        std::cout << "Run Planar Prior Assisted PatchMatch MVS ..." << std::endl;
        acmmp.SetPlanarPriorParams();

        const cv::Rect imageRC(0, 0, width, height);
        std::vector<cv::Point> support2DPoints;

        acmmp.GetSupportPoints(support2DPoints);
        const auto triangles = acmmp.DelaunayTriangulation(imageRC, support2DPoints);
        cv::Mat refImage = acmmp.GetReferenceImage().clone();
        std::vector<cv::Mat> mbgr(3);
        mbgr[0] = refImage.clone();
        mbgr[1] = refImage.clone();
        mbgr[2] = refImage.clone();
        cv::Mat srcImage;
        cv::merge(mbgr, srcImage);
        for (const auto triangle : triangles) {
            if (imageRC.contains(triangle.pt1) && imageRC.contains(triangle.pt2) && imageRC.contains(triangle.pt3)) {
                cv::line(srcImage, triangle.pt1, triangle.pt2, cv::Scalar(0, 0, 255));
                cv::line(srcImage, triangle.pt1, triangle.pt3, cv::Scalar(0, 0, 255));
                cv::line(srcImage, triangle.pt2, triangle.pt3, cv::Scalar(0, 0, 255));
            }
        }
        std::string triangulation_path = result_folder + "/triangulation.png";
        cv::imwrite(triangulation_path, srcImage);

        cv::Mat_<float> mask_tri = cv::Mat::zeros(height, width, CV_32FC1);
        std::vector<float4> planeParams_tri;
        planeParams_tri.clear();

        uint32_t idx = 0;
        for (const auto triangle : triangles) {
            if (imageRC.contains(triangle.pt1) && imageRC.contains(triangle.pt2) && imageRC.contains(triangle.pt3)) {
                float L01 = sqrt(pow(triangle.pt1.x - triangle.pt2.x, 2) + pow(triangle.pt1.y - triangle.pt2.y, 2));
                float L02 = sqrt(pow(triangle.pt1.x - triangle.pt3.x, 2) + pow(triangle.pt1.y - triangle.pt3.y, 2));
                float L12 = sqrt(pow(triangle.pt2.x - triangle.pt3.x, 2) + pow(triangle.pt2.y - triangle.pt3.y, 2));

                float max_edge_length = std::max(L01, std::max(L02, L12));
                float step = 1.0 / max_edge_length;

                for (float p = 0; p < 1.0; p += step) {
                    for (float q = 0; q < 1.0 - p; q += step) {
                        int x = p * triangle.pt1.x + q * triangle.pt2.x + (1.0 - p - q) * triangle.pt3.x;
                        int y = p * triangle.pt1.y + q * triangle.pt2.y + (1.0 - p - q) * triangle.pt3.y;
                        mask_tri(y, x) = idx + 1.0; // To distinguish from the label of non-triangulated areas
                    }
                }

                // estimate plane parameter
                float4 n4 = acmmp.GetPriorPlaneParams(triangle, depths);
                planeParams_tri.push_back(n4);
                idx++;
            }
        }

        cv::Mat_<float> priordepths = cv::Mat::zeros(height, width, CV_32FC1);
        for (int i = 0; i < width; ++i) {
            for (int j = 0; j < height; ++j) {
                if (mask_tri(j, i) > 0) {
                    float d = acmmp.GetDepthFromPlaneParam(planeParams_tri[mask_tri(j, i) - 1], i, j);
                    if (d <= acmmp.GetMaxDepth() && d >= acmmp.GetMinDepth()) {
                        priordepths(j, i) = d;
                    }
                    else {
                        mask_tri(j, i) = 0;
                    }
                }
            }
        }
        // std::string depth_path = result_folder + "/depths_prior.dmb";
        //  writeDepthDmb(depth_path, priordepths);

        acmmp.CudaPlanarPriorInitialization(planeParams_tri, mask_tri);
        acmmp.RunPatchMatch();

        for (int col = 0; col < width; ++col) {
            for (int row = 0; row < height; ++row) {
                int center = row * width + col;
                float4 plane_hypothesis = acmmp.GetPlaneHypothesis(center);
                depths(row, col) = plane_hypothesis.w;
                normals(row, col) = cv::Vec3f(plane_hypothesis.x, plane_hypothesis.y, plane_hypothesis.z);
                costs(row, col) = acmmp.GetCost(center);
            }
        }
    }

    std::string suffix = "/depths.dmb";
    if (geom_consistency) {
        suffix = "/depths_geom.dmb";
    }
    std::string depth_path = result_folder + suffix;
    std::string normal_path = result_folder + "/normals.dmb";
    std::string cost_path = result_folder + "/costs.dmb";
    writeDepthDmb(depth_path, depths);
    writeNormalDmb(normal_path, normals);
    writeDepthDmb(cost_path, costs);
    std::cout << "Processing image " << std::setw(8) << std::setfill('0') << problem.ref_image_id << " done!" << std::endl;
}

void JointBilateralUpsampling(const std::string &dense_folder, const Problem &problem, int acmmp_size)
{
    std::stringstream result_path;
    result_path << dense_folder << "/ACMMP" << "/2333_" << std::setw(8) << std::setfill('0') << problem.ref_image_id;
    std::string result_folder = result_path.str();
    std::string depth_path = result_folder + "/depths_geom.dmb";
    cv::Mat_<float> ref_depth;
    readDepthDmb(depth_path, ref_depth);

    std::string image_folder = dense_folder + std::string("/images");
    std::stringstream image_path;
    image_path << image_folder << "/" << std::setw(8) << std::setfill('0') << problem.ref_image_id << ".jpg";
    cv::Mat_<uint8_t> image_uint = cv::imread(image_path.str(), cv::IMREAD_GRAYSCALE);
    cv::Mat image_float;
    image_uint.convertTo(image_float, CV_32FC1);
    const float factor_x = static_cast<float>(acmmp_size) / image_float.cols;
    const float factor_y = static_cast<float>(acmmp_size) / image_float.rows;
    const float factor = std::min(factor_x, factor_y);

    const int new_cols = std::round(image_float.cols * factor);
    const int new_rows = std::round(image_float.rows * factor);
    cv::Mat scaled_image_float;
    cv::resize(image_float, scaled_image_float, cv::Size(new_cols,new_rows), 0, 0, cv::INTER_LINEAR);

    std::cout << "Run JBU for image " << problem.ref_image_id <<  ".jpg" << std::endl;
    RunJBU(scaled_image_float, ref_depth, dense_folder, problem );
}

void RunFusion(std::string &dense_folder, const std::vector<Problem> &problems, bool geom_consistency)
{
    size_t num_images = problems.size();
    std::string image_folder = dense_folder + std::string("/images");
    std::string cam_folder = dense_folder + std::string("/cams");

    std::vector<cv::Mat> images;
    std::vector<Camera> cameras;
    std::vector<cv::Mat_<float>> depths;
    std::vector<cv::Mat_<cv::Vec3f>> normals;
    std::vector<cv::Mat> masks;
    std::vector<cv::Mat> fusedepths;
    images.clear();
    cameras.clear();
    depths.clear();
    normals.clear();
    masks.clear();
    fusedepths.clear();
    
    std::map<int, int> image_id_2_index;

    for (size_t i = 0; i < num_images; ++i) {
        std::cout << "Reading image " << std::setw(8) << std::setfill('0') << i << "..." << std::endl;
        image_id_2_index[problems[i].ref_image_id] = i;
        std::stringstream image_path;
        image_path << image_folder << "/" << std::setw(8) << std::setfill('0') << problems[i].ref_image_id << ".jpg";
        cv::Mat_<cv::Vec3b> image = cv::imread (image_path.str(), cv::IMREAD_COLOR);
        std::stringstream cam_path;
        cam_path << cam_folder << "/" << std::setw(8) << std::setfill('0') << problems[i].ref_image_id << "_cam.txt";
        Camera camera = ReadCamera(cam_path.str());

        std::stringstream result_path;
        result_path << dense_folder << "/ACMMP" << "/2333_" << std::setw(8) << std::setfill('0') << problems[i].ref_image_id;
        std::string result_folder = result_path.str();
        std::string suffix = "/depths.dmb";
        if (geom_consistency) {
            suffix = "/depths_geom.dmb";
        }
        std::string depth_path = result_folder + suffix;
        std::string normal_path = result_folder + "/normals.dmb";
        cv::Mat_<float> depth;
        cv::Mat_<cv::Vec3f> normal;
        readDepthDmb(depth_path, depth);
        readNormalDmb(normal_path, normal);

        cv::Mat_<cv::Vec3b> scaled_image;
        RescaleImageAndCamera(image, scaled_image, depth, camera);
        images.push_back(scaled_image);
        cameras.push_back(camera);
        depths.push_back(depth);
        normals.push_back(normal);
        cv::Mat mask = cv::Mat::zeros(depth.rows, depth.cols, CV_8UC1);
        masks.push_back(mask);
        cv::Mat_<float> fusedepth = cv::Mat::zeros(depth.rows, depth.cols, CV_32FC1);   //初始化融合深度图
        fusedepths.push_back(fusedepth);
    }

    std::vector<PointList> PointCloud;
    PointCloud.clear();

    for (size_t i = 0; i < num_images; ++i) {
        std::cout << "Fusing image " << std::setw(8) << std::setfill('0') << i << "..." << std::endl;
        const int cols = depths[i].cols;
        const int rows = depths[i].rows;
        int num_ngb = problems[i].src_image_ids.size();
        std::vector<int2> used_list(num_ngb, make_int2(-1, -1));
        //改
        std::stringstream skyimg_path;
        skyimg_path << image_folder << "/sky/" << problems[i].ref_image_id << ".png";   //天空图像路径
        cv::Mat skyimg = cv::Mat::zeros(rows, cols, CV_8UC1);
        cv::Mat tmpimg = cv::imread(skyimg_path.str(), 0);
        if(!tmpimg.empty()){
            skyimg = tmpimg;
        }
        cv::resize(skyimg,skyimg,cv::Size(cols/4*4,rows/4*4));
        cv::threshold(skyimg,skyimg,150,255,cv::THRESH_BINARY);//改

        for (int r =0; r < rows; ++r) {
            for (int c = 0; c < cols; ++c) {
                /**改**/
                 if(skyimg.at<uchar>(r,c) > 150){
                    depths[i].at<float>(r, c) = 0;
                    continue;
                }
               /**改**/
                if (masks[i].at<uchar>(r, c) == 1)
                    continue;
                float ref_depth = depths[i].at<float>(r, c);
                cv::Vec3f ref_normal = normals[i].at<cv::Vec3f>(r, c);

                if (ref_depth <= 0.0)
                    continue;

                float3 PointX = Get3DPointonWorld(c, r, ref_depth, cameras[i]);
                float3 consistent_Point = PointX;
                cv::Vec3f consistent_normal = ref_normal;
                float consistent_Color[3] = {(float)images[i].at<cv::Vec3b>(r, c)[0], (float)images[i].at<cv::Vec3b>(r, c)[1], (float)images[i].at<cv::Vec3b>(r, c)[2]};
                int num_consistent = 0;
                float dynamic_consistency = 0;

                for (int j = 0; j < num_ngb; ++j) {
                    int src_id = image_id_2_index[problems[i].src_image_ids[j]];
                    const int src_cols = depths[src_id].cols;
                    const int src_rows = depths[src_id].rows;
                    float2 point;
                    float proj_depth;
                    ProjectonCamera(PointX, cameras[src_id], point, proj_depth);
                    int src_r = int(point.y + 0.5f);
                    int src_c = int(point.x + 0.5f);
                    if (src_c >= 0 && src_c < src_cols && src_r >= 0 && src_r < src_rows) {
                        if (masks[src_id].at<uchar>(src_r, src_c) == 1)
                            continue;

                        float src_depth = depths[src_id].at<float>(src_r, src_c);
                        cv::Vec3f src_normal = normals[src_id].at<cv::Vec3f>(src_r, src_c);
                        if (src_depth <= 0.0)
                            continue;

                        float3 tmp_X = Get3DPointonWorld(src_c, src_r, src_depth, cameras[src_id]);
                        float2 tmp_pt;
                        ProjectonCamera(tmp_X, cameras[i], tmp_pt, proj_depth);
                        float reproj_error = sqrt(pow(c - tmp_pt.x, 2) + pow(r - tmp_pt.y, 2));
                        float relative_depth_diff = fabs(proj_depth - ref_depth) / ref_depth;
                        float angle = GetAngle(ref_normal, src_normal);
                        //改
                        // if (reproj_error < 4.0f && relative_depth_diff < 0.01f && angle < 0.25f) {  //满足这些条件的    //10 174533   20 349066   25 0.436333
                        if (reproj_error < 2.0f && relative_depth_diff < 0.01f && angle < 0.174533f) {
                           /* consistent_Point.x += tmp_X.x;
                            consistent_Point.y += tmp_X.y;
                            consistent_Point.z += tmp_X.z;
                            consistent_normal = consistent_normal + src_normal;
                            consistent_Color[0] += images[src_id].at<cv::Vec3b>(src_r, src_c)[0];
                            consistent_Color[1] += images[src_id].at<cv::Vec3b>(src_r, src_c)[1];
                            consistent_Color[2] += images[src_id].at<cv::Vec3b>(src_r, src_c)[2];*/
                           

                            used_list[j].x = src_c;
                            used_list[j].y = src_r;

                            float tmp_index = reproj_error + 200 * relative_depth_diff + angle * 10;
                            float cons = exp(-tmp_index);
                            dynamic_consistency += exp(-tmp_index);
                            num_consistent++;
                        }
                    }
                }

                if (num_consistent >= 1 && (dynamic_consistency > 0.3 * num_consistent)) {
                    /*consistent_Point.x /= (num_consistent + 1.0f);
                    consistent_Point.y /= (num_consistent + 1.0f);
                    consistent_Point.z /= (num_consistent + 1.0f);
                    consistent_normal /= (num_consistent + 1.0f);
                    consistent_Color[2] /= (num_consistent + 1.0f);*/

                    PointList point3D;
                    point3D.coord = consistent_Point;
                    point3D.normal = make_float3(consistent_normal[0], consistent_normal[1], consistent_normal[2]);
                    point3D.color = make_float3(consistent_Color[0], consistent_Color[1], consistent_Color[2]);
                    PointCloud.push_back(point3D);
                    fusedepths[i].at<float>(r,c) = depths[i].at<float>(r,c);
                    for (int j = 0; j < num_ngb; ++j) {
                        if (used_list[j].x == -1)
                            continue;
                        masks[image_id_2_index[problems[i].src_image_ids[j]]].at<uchar>(used_list[j].y, used_list[j].x) = 1;
                        fusedepths[problems[i].src_image_ids[j]].at<float>(used_list[j].y, used_list[j].x) = depths[problems[i].src_image_ids[j]].at<float>(used_list[j].y, used_list[j].x);
                    }
                }
            }
        }
    }
    for (size_t i = 0; i < num_images; ++i) {    //遍历所有图像
            std::string propagate_path = dense_folder + "/ACMMP/depth" + std::to_string(problems[i].ref_image_id);    //传播深度图保存路径
            std::string fuse_path = dense_folder + "/ACMMP/fuse" + std::to_string(problems[i].ref_image_id);          //融合深度图保存路径
            ExportDepthMapByJetColormap(propagate_path,depths[i]);  //输出传播深度图
            ExportDepthMapByJetColormap(fuse_path,fusedepths[i]);   //输出融合深度图
        }

    std::string ply_path = dense_folder + "/ACMMP/ACMMP_model.ply";
    StoreColorPlyFileBinaryPointCloud (ply_path, PointCloud);
}

//改
//置信度计算函数
void EstimateConfidence(const std::string &dense_folder, const std::vector<Problem> &problems, const int idx, bool multi_geometry, bool fill_confestimate)
{
    const Problem problem = problems[idx];
    if(!fill_confestimate)
        std::cout << "Estimate Confidence "<< std::setw(2) << std::setfill('0') << problem.ref_image_id << " ... " ;
    else    
        std::cout << "Fill Estimate Confidence "<< std::setw(2) << std::setfill('0') << problem.ref_image_id << " ... " ;
    cudaSetDevice(0);
    std::stringstream result_path;
    result_path << dense_folder << "/ACMMP" << "/2333_" << std::setw(8) << std::setfill('0') << problem.ref_image_id;
    std::string result_folder = result_path.str();

    ACMMP acmmp;
    acmmp.SetConfidenceParams(fill_confestimate);        //补全置信度计算参数
    acmmp.SetGeomConsistencyParams(multi_geometry);      //几何一致性参数
    // acmmp.InuputInitialization(dense_folder, problem);
    acmmp.InuputInitialization(dense_folder, problems, idx);   //输入初始化        //凶手1:邻域帧dmb
    acmmp.CudaPlanarPriorInitialization_fill(dense_folder, problem); 
    acmmp.ConfidenceEstimation();
    
    const int width = acmmp.GetReferenceImageWidth();
    const int height = acmmp.GetReferenceImageHeight();

    cv::Mat_<float> mrfs = cv::Mat::zeros(height, width, CV_32FC1);

    for (int col = 0; col < width; ++col) {
        for (int row = 0; row < height; ++row) {
            int center = row * width + col;
            float mrf = acmmp.GetMRF(center);
            mrfs(row, col) = mrf;
        }
    }

    if(fill_confestimate)
    {
        std::string filldepth_path = result_folder + "/depths_fill.dmb";
        std::string fillnormal_path = result_folder + "/normals_fill.dmb";
        std::string mrf_path = result_folder + "/mrfs.dmb";
        std::string suffix = "/depths.dmb";
        if (multi_geometry) {
            suffix = "/depths_geom.dmb";
        }
        std::string depth_path = result_folder + suffix;
        std::string normal_path = result_folder + "/normals.dmb";

        cv::Mat_<float> depths = cv::Mat::zeros(height, width, CV_32FC1);
        cv::Mat_<cv::Vec3f> normals = cv::Mat::zeros(height, width, CV_32FC3);

        cv::Mat_<float> depth;
        cv::Mat_<cv::Vec3f> normal;
        cv::Mat_<float> fill_depth;
        cv::Mat_<cv::Vec3f> fill_normal;
        cv::Mat_<float> mrf;
        readDepthDmb(filldepth_path, fill_depth);       //读填补深度dmb
        readNormalDmb(fillnormal_path, fill_normal);    //读填补法向量dmb
        readDepthDmb(mrf_path, mrf);                    //读mrf的dmb     
        readDepthDmb(depth_path, depth);                //读深度dmb
        readNormalDmb(normal_path, normal);             //读法向量dmb

        for (int col = 0; col < width; ++col) {
            for (int row = 0; row < height; ++row) {

                if(mrfs.at<float>(row, col) > mrf.at<float>(row, col)){
                    depths(row, col) = fill_depth.at<float>(row, col);
                    normals(row, col) = fill_normal.at<cv::Vec3f>(row, col);
                }
                else{
                    depths(row, col) = depth.at<float>(row, col);
                    normals(row, col) = normal.at<cv::Vec3f>(row, col);
                }
            }
        }
        //保存dmb文件
        writeDepthDmb(depth_path, depths);
        writeNormalDmb(normal_path, normals);
    }


    //保存dmb文件
    std::string mrf_path = result_folder + "/mrfs.dmb";
    writeDepthDmb(mrf_path, mrfs);
    
    std::cout << "Done"<<std::endl;
}

//三角插值补全函数//改
void TriangleFill(const std::string &dense_folder, const Problem &problem, bool geom_consistency)
{
    std::cout<<"Triangle Fill "<< std::setw(2) << std::setfill('0') << problem.ref_image_id<<" ... ";
    std::string image_folder = dense_folder + std::string("/images");       //图片路径
    std::stringstream image_path;
    image_path << image_folder << "/" << std::setw(8) << std::setfill('0') << problem.ref_image_id << ".jpg";    
    cv::Mat_<cv::Vec3b> image = cv::imread(image_path.str(), cv::IMREAD_COLOR);  
    cv::Mat_<uint8_t> image_uint = cv::imread(image_path.str(), cv::IMREAD_GRAYSCALE);              //读取当前帧图像
    cv::Mat image_float;
    image_uint.convertTo(image_float, CV_32FC1);
    std::stringstream result_path;
    result_path << dense_folder << "/ACMMP" << "/2333_" << std::setw(8) << std::setfill('0') << problem.ref_image_id;    //dmb路径以及保存路径
    std::string result_folder = result_path.str();
    std::string suffix = "/depths.dmb";
    if (geom_consistency) {
        suffix = "/depths_geom.dmb";
    }
    std::string depth_path = result_folder + suffix;
    std::string mrf_path = result_folder + "/mrfs.dmb";
    std::string normal_path = result_folder + "/normals.dmb";
    cv::Mat_<float> depth;
    cv::Mat_<float> mrf;
    cv::Mat_<cv::Vec3f> normal;
    readDepthDmb(depth_path, depth);    //读深度值dmb
    readDepthDmb(mrf_path, mrf);        //读mrf的dmb
    readNormalDmb(normal_path, normal);             //读法向量dmb
    std::stringstream cam_path;
    std::string cam_folder = dense_folder + std::string("/cams");
    cam_path << cam_folder << "/" << std::setw(8) << std::setfill('0') << problem.ref_image_id << "_cam.txt";   //相机参数路径
    Camera camera = ReadCamera(cam_path.str());
    cv::Mat_<cv::Vec3b> scaled_image;
    RescaleImageAndCamera(image, scaled_image, depth, camera);  //resize图像和相机参数

    const int cols = depth.cols;                //图像宽    x列
    const int rows = depth.rows;                //图像高    y行 
    const cv::Rect imageRC(0, 0, cols, rows);
    std::vector<cv::Point> support2DPoints;     //装三角插值基点的容器
    int step_size = 5;  //小窗口步长
    for (int col =0; col < cols; col += step_size) {        //x列       //遍历所有像素
        for (int row = 0; row < rows; row += step_size) {   //y行    
            cv::Point temp_point(-1,-1);
            int c_bound = std::min(cols, col + step_size);  //高    x列 
            int r_bound = std::min(rows, row + step_size);  //宽    y行
            for (int c = col; c < c_bound; c++) {           //x列
                for (int r = row; r < r_bound; r++) {       //y行
                    float tmpmrf = 0;
                    if (mrf.at<float>(r,c) > tmpmrf) {      //每个小窗口只要置信度最大的一个点
                        temp_point = cv::Point(c,r);
                        tmpmrf = mrf.at<float>(r,c);
                    }
                }
            }
            if(mrf.at<float>(temp_point.y,temp_point.x) > 0.7f && temp_point.x>0) {      //选取置信度高的点作为三角插值的基点    canshu
                support2DPoints.push_back(temp_point);
            }
        }
    }
    
    if (support2DPoints.empty()) {
        std::cout<<"empty "<<problem.ref_image_id<<std::endl;
        return;
    }
    std::vector<Triangle> triangles;
    std::vector<cv::Vec6f> temp_triangles;
    cv::Subdiv2D subdiv2d(imageRC);
    for (const auto point : support2DPoints) {
        subdiv2d.insert(cv::Point2f((float)point.x, (float)point.y));
    }
    subdiv2d.getTriangleList(temp_triangles);
    for (const auto temp_vec : temp_triangles) {
        cv::Point pt1((int)temp_vec[0], (int)temp_vec[1]);
        cv::Point pt2((int)temp_vec[2], (int)temp_vec[3]);
        cv::Point pt3((int)temp_vec[4], (int)temp_vec[5]);
        triangles.push_back(Triangle(pt1, pt2, pt3));
    }
    cv::resize(image_float, image_float, cv::Size(cols,rows), 0, 0, cv::INTER_LINEAR);
    cv::Mat refImage =  image_float.clone();    //复制图像
    std::vector<cv::Mat> mbgr(3);               //输出三角插值连线用的
    mbgr[0] = refImage.clone();
    mbgr[1] = refImage.clone();
    mbgr[2] = refImage.clone();
    cv::Mat srcImage;
    cv::merge(mbgr, srcImage);
    for (const auto triangle : triangles) {     
        if (imageRC.contains(triangle.pt1) && imageRC.contains(triangle.pt2) && imageRC.contains(triangle.pt3)) {
            cv::line(srcImage, triangle.pt1, triangle.pt2, cv::Scalar(0, 0, 255));
            cv::line(srcImage, triangle.pt1, triangle.pt3, cv::Scalar(0, 0, 255));
            cv::line(srcImage, triangle.pt2, triangle.pt3, cv::Scalar(0, 0, 255));
        }
    }
    std::string triangulation_path = result_folder + "/triangulation.png";  
    cv::imwrite(triangulation_path, srcImage);      //输出三角插值连线图
    cv::Mat_<float> mask_tri = cv::Mat::zeros(rows, cols, CV_32FC1);
    std::vector<float4> planeParams_tri;    
    uint32_t idx = 0;
    for (const auto triangle : triangles) {         //三角插值
        if (imageRC.contains(triangle.pt1) && imageRC.contains(triangle.pt2) && imageRC.contains(triangle.pt3)) {
            float L01 = sqrt(pow(triangle.pt1.x - triangle.pt2.x, 2) + pow(triangle.pt1.y - triangle.pt2.y, 2));
            float L02 = sqrt(pow(triangle.pt1.x - triangle.pt3.x, 2) + pow(triangle.pt1.y - triangle.pt3.y, 2));
            float L12 = sqrt(pow(triangle.pt2.x - triangle.pt3.x, 2) + pow(triangle.pt2.y - triangle.pt3.y, 2));
            float max_edge_length = std::max(L01, std::max(L02, L12));
            float step = 1.0 / max_edge_length;
            for (float p = 0; p < 1.0; p += step) {
                for (float q = 0; q < 1.0 - p; q += step) {
                    int x = p * triangle.pt1.x + q * triangle.pt2.x + (1.0 - p - q) * triangle.pt3.x;
                    int y = p * triangle.pt1.y + q * triangle.pt2.y + (1.0 - p - q) * triangle.pt3.y;
                    mask_tri(y, x) = idx + 1.0;     //大于0说明是有三角插值先验的区域   //这一步后正常来说是全部点都不为0的
                }
            }
            cv::Mat A(3, 4, CV_32FC1);
            cv::Mat B(4, 1, CV_32FC1);
            float3 ptX1 = Get3DPointonRefCam(triangle.pt1.x, triangle.pt1.y, depth.at<float>(triangle.pt1.y, triangle.pt1.x), camera);
            float3 ptX2 = Get3DPointonRefCam(triangle.pt2.x, triangle.pt2.y, depth.at<float>(triangle.pt2.y, triangle.pt2.x), camera);
            float3 ptX3 = Get3DPointonRefCam(triangle.pt3.x, triangle.pt3.y, depth.at<float>(triangle.pt3.y, triangle.pt3.x), camera);
            A.at<float>(0, 0) = ptX1.x;
            A.at<float>(0, 1) = ptX1.y;
            A.at<float>(0, 2) = ptX1.z;
            A.at<float>(0, 3) = 1.0;
            A.at<float>(1, 0) = ptX2.x;
            A.at<float>(1, 1) = ptX2.y;
            A.at<float>(1, 2) = ptX2.z;
            A.at<float>(1, 3) = 1.0;
            A.at<float>(2, 0) = ptX3.x;
            A.at<float>(2, 1) = ptX3.y;
            A.at<float>(2, 2) = ptX3.z;
            A.at<float>(2, 3) = 1.0;
            cv::SVD::solveZ(A, B);
            float4 n4 = make_float4(B.at<float>(0, 0), B.at<float>(1, 0), B.at<float>(2, 0), B.at<float>(3, 0));
            float norm2 = sqrt(pow(n4.x, 2) + pow(n4.y, 2) + pow(n4.z, 2));
            if (n4.w < 0) {
                norm2 *= -1;
            }
            n4.x /= norm2;
            n4.y /= norm2;
            n4.z /= norm2;
            n4.w /= norm2;
            planeParams_tri.push_back(n4);
            idx++;
        }
    }
    cv::Mat_<float> filldepth = cv::Mat::zeros(rows, cols, CV_32FC1);       //初始化深度图
    cv::Mat_<cv::Vec3f> fillnormal =  cv::Mat::zeros(rows, cols, CV_32FC3); //初始化法向量图
    for (int c = 0; c < cols; ++c) {                                        //遍历所有像素
        for (int j = 0; j < rows; ++j) {

            float confidece = mrf.at<float>(j,c);
            if(confidece > 0.7f){
                filldepth(j, c) = depth.at<float>(j,c);
                fillnormal(j, c) = normal.at<cv::Vec3f>(j,c);
            }
            else{
                filldepth(j, c) = 0;
                fillnormal(j, c) = cv::Vec3f(0,0,0);
            }


            if (mask_tri(j, c) > 0) {           //如果是三角插值区域
                float d = -planeParams_tri[mask_tri(j, c)-1].w*camera.K[0]/((c-camera.K[2])*planeParams_tri[mask_tri(j, c)-1].x+(camera.K[0]/camera.K[4])*(j-camera.K[5])*planeParams_tri[mask_tri(j, c)-1].y+camera.K[0]*planeParams_tri[mask_tri(j, c)-1].z);
                if (d >0) {                     //如果先验深度在范围内
                    filldepth(j, c) = d;        //先验深度赋值
                    fillnormal(j, c) = cv::Vec3f(planeParams_tri[mask_tri(j, c) - 1].x, planeParams_tri[mask_tri(j, c) - 1].y, planeParams_tri[mask_tri(j, c) - 1].z);  //法向量  
                }
                else {                          //如果先验深度不在范围内
                    mask_tri(j, c) = 0;         //标记为没有先验的点            //先验图里的黑点基本就是这些区域
                }
            }
        }
    }
    // 清空容器
    std::vector<cv::Point>().swap(support2DPoints);
    std::vector<Triangle>().swap(triangles);
    std::vector<cv::Vec6f>().swap(temp_triangles);
    std::vector<cv::Mat>().swap(mbgr);
    std::vector<float4>().swap(planeParams_tri);    
    ExportDepthMapByJetColormap(result_folder + ".fill", filldepth);    //输出填补深度图
    std::string filldepth_path = result_folder + "/depths_fill.dmb";
    std::string fillnormal_path = result_folder + "/normals_fill.dmb";
    writeDepthDmb(filldepth_path, filldepth);                           //保存填补深度
    writeNormalDmb(fillnormal_path, fillnormal);                        //保存填补法向量
    
    std::cout<<"Done "<<std::endl;
}


//主函数
int main(int argc, char** argv)
{
    if (argc < 2) {
        std::cout << "USAGE: ACMMP dense_folder" << std::endl;
        return -1;
    }

    std::string dense_folder = argv[1];
    std::vector<Problem> problems;
    GenerateSampleList(dense_folder, problems);

    std::string output_folder = dense_folder + std::string("/ACMMP");
    mkdir(output_folder.c_str(), 0777);

    size_t num_images = problems.size();

    float time;
    clock_t start,end;
    start = clock();

    std::cout << "There are " << num_images << " problems needed to be processed!" << std::endl;

    int max_num_downscale = ComputeMultiScaleSettings(dense_folder, problems);

     int flag = 0;
     int geom_iterations = 2;
     bool geom_consistency = false;
     bool planar_prior = false;
     bool hierarchy = false;
     bool multi_geometry = false;
     //改
    bool fill_confestimate = false;     //填补置信度计算标志位
     while (max_num_downscale >= 0) {
        std::cout << "Scale: " << max_num_downscale << std::endl;

        for (size_t i = 0; i < num_images; ++i) {
            if (problems[i].num_downscale >= 0) {
                problems[i].cur_image_size = problems[i].max_image_size / pow(2, problems[i].num_downscale);
                problems[i].num_downscale--;
            }
        }

        if (flag == 0) {
             std::cout << "-----------------------------flag is --------------------------------" << flag << std::endl;
            flag = 1;
            geom_consistency = false;
            planar_prior = true;
            for (size_t i = 0; i < num_images; ++i) {
                ProcessProblem(dense_folder, problems, i, geom_consistency, planar_prior, hierarchy);
            }
            //改
            for (size_t i = 0; i < num_images; ++i) {
                EstimateConfidence(dense_folder, problems, i, multi_geometry, fill_confestimate); //原估计置信度计算
            }
            for (size_t i = 0; i < num_images; ++i) {
                TriangleFill(dense_folder, problems[i], geom_consistency);                      //三角插值补全
            }
            fill_confestimate = true;   //计算填补置信度
            for (size_t i = 0; i < num_images; ++i) {
                EstimateConfidence(dense_folder,problems, i, multi_geometry, fill_confestimate); //三角插值置信度计算并与原估计比较得到输出
            }            //改
            geom_consistency = true;
            // planar_prior = false;
            for (int geom_iter = 0; geom_iter < geom_iterations; ++geom_iter) {
                if (geom_iter == 0) {
                    multi_geometry = false;
                }
                else {
                    multi_geometry = true;
                }
                 //改
                if(geom_iter != geom_iterations - 1){
                    planar_prior = true;
                } else {
                    planar_prior = false;
                }//改

                for (size_t i = 0; i < num_images; ++i) {
                    ProcessProblem(dense_folder,  problems, i, geom_consistency, planar_prior, hierarchy, multi_geometry);
                }
                bool fill_confestimate = false;     //填补置信度计算标志位
                //改
                for (size_t i = 0; i < num_images; ++i) {
                    EstimateConfidence(dense_folder, problems, i, multi_geometry, fill_confestimate); //原估计置信度计算
                }
                for (size_t i = 0; i < num_images; ++i) {
                    TriangleFill(dense_folder, problems[i], geom_consistency);                      //三角插值补全
                }
                fill_confestimate = true;   //计算填补置信度
                for (size_t i = 0; i < num_images; ++i) {
                    EstimateConfidence(dense_folder, problems, i, multi_geometry, fill_confestimate); //三角插值置信度计算并与原估计比较得到输出
                }            //改
            }
                // //改
                // bool fill_confestimate = false;     //填补置信度计算标志位
                // for (size_t i = 0; i < num_images; ++i) {
                //     EstimateConfidence(dense_folder, problems, i, multi_geometry, fill_confestimate); //原估计置信度计算
                // }
                // for (size_t i = 0; i < num_images; ++i) {
                //     TriangleFill(dense_folder, problems[i], geom_consistency);                      //三角插值补全
                // }
                // fill_confestimate = true;   //计算填补置信度
                // for (size_t i = 0; i < num_images; ++i) {
                //     EstimateConfidence(dense_folder, problems, i, multi_geometry, fill_confestimate); //三角插值置信度计算并与原估计比较得到输出
                // }            //改
        }
        else {
            std::cout << "----------------------------flag is -------------------------" << flag << std::endl;
            for (size_t i = 0; i < num_images; ++i) {
               JointBilateralUpsampling(dense_folder, problems[i], problems[i].cur_image_size);
             }

            hierarchy = true;
            geom_consistency = false;
            planar_prior = true;
            for (size_t i = 0; i < num_images; ++i) {
                ProcessProblem(dense_folder,  problems, i, geom_consistency, planar_prior, hierarchy);
            }
            //改
            bool fill_confestimate = false;     //填补置信度计算标志位
            for (size_t i = 0; i < num_images; ++i) {
                EstimateConfidence(dense_folder,problems, i, multi_geometry, fill_confestimate);   //原估计置信度计算
            }
            for (size_t i = 0; i < num_images; ++i) {
                TriangleFill(dense_folder, problems[i], geom_consistency);                          //三角插值补全
            }
            fill_confestimate = true;   //计算填补置信度
            for (size_t i = 0; i < num_images; ++i) {
                EstimateConfidence(dense_folder, problems, i, multi_geometry, fill_confestimate);   //三角插值置信度计算并与原估计比较得到输出
            }//改

            hierarchy = false;
            geom_consistency = true;
            // planar_prior = false;
            for (int geom_iter = 0; geom_iter < geom_iterations; ++geom_iter) {
                if (geom_iter == 0) {
                    multi_geometry = false;
                }
                else {
                    multi_geometry = true;
                }
                //改
                if(geom_iter != geom_iterations - 1){
                    planar_prior = true;
                } else {
                    planar_prior = false;
                }//改
                for (size_t i = 0; i < num_images; ++i) {
                    ProcessProblem(dense_folder,  problems, i, geom_consistency, planar_prior, hierarchy, multi_geometry);
                }
                //改
                bool fill_confestimate = false;     //填补置信度计算标志位
                for (size_t i = 0; i < num_images; ++i) {
                    EstimateConfidence(dense_folder,problems, i, multi_geometry, fill_confestimate);   //原估计置信度计算
                }
                for (size_t i = 0; i < num_images; ++i) {
                    TriangleFill(dense_folder, problems[i], geom_consistency);                          //三角插值补全
                }
                fill_confestimate = true;   //计算填补置信度
                for (size_t i = 0; i < num_images; ++i) {
                    EstimateConfidence(dense_folder, problems, i, multi_geometry, fill_confestimate);   //三角插值置信度计算并与原估计比较得到输出
                }//改
            }
                //  //改
                // bool fill_confestimate = false;     //填补置信度计算标志位
                // for (size_t i = 0; i < num_images; ++i) {
                //     EstimateConfidence(dense_folder,problems, i, multi_geometry, fill_confestimate);   //原估计置信度计算
                // }
                // for (size_t i = 0; i < num_images; ++i) {
                //     TriangleFill(dense_folder, problems[i], geom_consistency);                          //三角插值补全
                // }
                // fill_confestimate = true;   //计算填补置信度
                // for (size_t i = 0; i < num_images; ++i) {
                //     EstimateConfidence(dense_folder, problems, i, multi_geometry, fill_confestimate);   //三角插值置信度计算并与原估计比较得到输出
                // }//改

                //改
                // multi_geometry = true;
                // for (size_t i = 0; i < num_images; ++i) {
                //     ProcessProblem(dense_folder,  problems, i, geom_consistency, planar_prior, hierarchy, multi_geometry);
                // }

        }

        max_num_downscale--;
    }
    //改
    geom_consistency = true;
    end = clock();
    time = (float)(end-start);                      //时间
    int min = ((time/CLOCKS_PER_SEC)/60);           //分
    float second = (time/CLOCKS_PER_SEC)-min*60;    //秒
    std::cout<<"Dense Reconstruction Time = "<<min<<" min "<<second<<" s"<<std::endl<<std::endl;    //输出时间
    RunFusion(dense_folder, problems, geom_consistency);

    return 0;
}
